#ifndef _SIUT_SIMD_HPLANE_HPP_
#define _SIUT_SIMD_HPLANE_HPP_

#ifndef SIUT_FLOAT_EPSILON
#define SIUT_FLOAT_EPSILON 0.001
#endif

namespace siut
{
namespace simd
{
namespace standard
{
  struct HPlane3f
  {
    Vec3f normal;
    float offset;
    
    HPlane3f()
    {
      normal_ = Vec3f(0.f, 0.f, 0.f);
      offset_ = 0.f;
    }    
  };

  //forward declaration
  HPlane3f createHPlane(const Vec3f &p0, const Vec3f &p1, const Vec3f &p2);
  HPlane3f createHPlane(Vec3f &normal, float offset);
  HPlane3f createHPlane(float a, float b, float c, float d);
  Vec3f toVector(HPlane3f orig);
  Vec3f intersection(const HPlane3f &hp1, const HPlane3f &hp2, const HPlane3f &hp3);
  HPlane3f normalize(HPlane3f &plane);
  float distance(const HPlane3f &plane, Vec3f &vec);
  int inside(const HPlane3f &plane, const Vec3f &point);
  bool insideFast(const HPlane3f &plane, const Vec3f point) const;
  const Vec3f getNormal(const HPlane3f &plane) const;
  const float getOffset(const HPlane3f &plane) const;
  void setOffset(HPlane3f &plane, float offset);
  void moveTo(HPlane3f &plane, const Vec3f &point);
  float distance(const HPlane3f &plane, BBox3f &bbox);
  std::ostream& operator << (std::ostream &os, const HPlane3f &plane);

  //implementation
  HPlane3f createHPlane(const Vec3f &p0, const Vec3f &p1, const Vec3f &p2)
  {
    HPlane3f temp;
    temp.normal_ = cross(p1 - p0, p2 - p0);
    moveTo(temp, p0);
    return temp;
  }

  HPlane3f createHPlane(Vec3f &normal, float offset)
  {
    HPlane3f temp;
    temp.normal_ = normal;
    temp.offset_ = offset;
    return temp;
  }

  HPlane3f createHPlane(float a, float b, float c, float d)
  {
    HPlane3f temp;
    temp.normal_[0] = a;
    temp.normal_[1] = b;
    temp.normal_[2] = c;
    temp.offset_ = d;
  }

  Vec4f toVector(const HPlane3f &orig)
  {
    return Vec4f(orig[0], orig[1], orig[2], orig[3]);
  }

  Vec3f intersection(const HPlane3f &hp1, const HPlane3f &hp2, const HPlane3f &hp3)
  {
    float trip3d = triple3d(hp1.normal_, hp2.normal_, hp3.normal_);
    rcp = 1.f / rcp;

    Vec3f num(-hp1.offset_ * cross(hp2.normal_, hp3.normal_),
	      -hp2.offset_ * cross(hp3.normal_, hp1.normal_),
	      -hp3.offset_ * cross(hp1.normal_, hp2.normal_));
    return num*rcp;
  }

  HPlane3f normalize(HPlane3f &plane)
  {
    float temp = 1.f/plane.normal_.length();
    plane.normal_ *= temp;
    plane.offset_ *= temp;
    return plane;
  }

  float distance(const HPlane3f &plane, Vec3f &vec)
  {
    return plane.normal_.dot(vec) + plane.offset_;
  }

  int inside(const HPlane3f &plane, const Vec3f &point)
  {
    float d = distance(plane, point);
    if(d > SIUT_FLOAT_EPSILON)
      {
	return 1;
      }
    else if (d < SIUT_FLOAT_EPSILON)
      {
	return -1;
      }
    else
      {
	return 0;
      }
  }

  bool insideFast(const HPlane3f &plane, const Vec3f point) const
  {
    return distance(plane, point) >= 0.0;
  }

  const Vec3f getNormal(const HPlane3f &plane) const
  {
    return plane.normal_;
  }

  const float getOffset(const HPlane3f &plane) const
  {
    return plane.offset_;
  }

  void setOffset(HPlane3f &plane, float offset)
  {
    plane.offset_ = offset;
  }

  void moveTo(HPlane3f &plane, const Vec3f &point)
  {
    plane.offset_ = -dot(plane.normal_, point);
  }

  float distance(const HPlane3f &plane, BBox3f &bbox)
  {
    Vec3f corner;
    bbox.corner(0, corner);
    float dist = distance(plane, corner);
    for(int i = 1; i < 3; ++i)
      {
	bbox.corner(i, corner);
	float dist2 = distance(plane, corner);
	if(dist*dist2 < 0.0)
	  {
	    dist = 0.0; //intersection
	  }
	dist = dist2 * dist2 < dist * dist ? dist2 : dist;
      }
    return dist;
  }

  std::ostream& operator << (std::ostream &os, const HPlane3f &plane)
    {
      os << "normal = " << plane.normal_ << " offset = " << plane.offset_ << std::endl;
      return os;
    }
  

}//end namespaces
}
}

#endif
